﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace SmartMathLibrary
{
    /// <summary>
    /// This class provides a singular value decomposition for real matrices.
    /// </summary>
    [Serializable]
    public class SingularValueDecomposition : AbstractDecomposition
    {
        /// <summary>
        /// Saves the vector s as a result of the decomposition.
        /// </summary>
        private double[] s;

        /// <summary>
        /// Saves the matrix data of the v matrix as a result of the decomposition.
        /// </summary>
        private double[,] v;

        /// <summary>
        /// Saves the matrix data of the u matrix as a result of the decomposition.
        /// </summary>
        private double[,] u;

        /// <summary>
        /// Describes if the matrix had to be transposed for the algorithm.
        /// </summary>
        private bool transposed;

        /// <summary>
        /// The source matrix for the decomposition.
        /// </summary>
        private Matrix sourceMatrix;

        /// <summary>
        /// Initializes a new instance of the <see cref="SingularValueDecomposition"/> class.
        /// </summary>
        /// <param name="sourceMatrix">The source matrix for the decomposition.</param>
        public SingularValueDecomposition(Matrix sourceMatrix)
        {
            if (sourceMatrix == (Matrix) null)
            {
                throw new ArgumentNullException("sourceMatrix");
            }

            this.transposed = false;
            this.sourceMatrix = sourceMatrix;
        }

        /// <summary>
        /// Gets the S vector as a result of the decomposition.
        /// </summary>
        /// <value>The S vector as a result of the decomposition.</value>
        public GeneralVector S
        {
            get
            {
                //if (this.transposed)
                //{
                //    double[] tempuri = new double[this.s.Length];

                //    for (int i = 0; i < this.s.Length; i++)
                //    {
                //        tempuri[this.s.Length - i - 1] = this.s[i];
                //    }

                //    return new GeneralVector(tempuri);
                //}

                return new GeneralVector(this.s);
            }
        }

        /// <summary>
        /// Gets the matrix V as a result of the decomposition.
        /// </summary>
        /// <value>The matrix V as a result of the decomposition.</value>
        public Matrix V
        {
            get
            {
                if (this.transposed)
                {
                    return (new Matrix(this.u));
                }

                return new Matrix(this.v);
            }
        }

        /// <summary>
        /// Gets the matrix U as a result of the decomposition.
        /// </summary>
        /// <value>The matrix U as a result of the decomposition.</value>
        public Matrix U
        {
            get
            {
                if (this.transposed)
                {
                    return (new Matrix(this.v));
                }

                return new Matrix(this.u);
            }
        }

        /// <summary>
        /// Gets or sets the source matrix for the decomposition.
        /// </summary>
        /// <value>The source matrix for the decomposition.</value>
        public Matrix SourceMatrix
        {
            get { return sourceMatrix; }
            set { sourceMatrix = value; }
        }

        /// <summary>
        /// Describes if the matrix had to be transposed for the algorithm.
        /// </summary>
        public bool Transposed
        {
            get { return transposed; }
        }

        /// <summary>
        /// Executes the singular value decomposition.
        /// </summary>
        public override void ExecuteDecomposition()
        {
            this.ExecuteDecomposition(true, true);
        }

        /// <summary>
        /// Executes the singular value decomposition.
        /// </summary>
        /// <param name="withU">If set to true the algorithm will compute U.</param>
        /// <param name="withV">If set to true the algorithm will compute V.</param>
        public void ExecuteDecomposition(bool withU, bool withV)
        {
            // Derived from LINPACK code.
            double[,] a = this.sourceMatrix.Copy().MatrixData;
            int m = this.sourceMatrix.Rows;
            int n = this.sourceMatrix.Columns;

            this.transposed = false;

            if (m < n)
            {
                this.transposed = true;
                a = this.sourceMatrix.Copy().Transpose().MatrixData;
                ExtendedMath.Swap(ref n, ref m);
            }

            int nu = Math.Min(m, n);
            this.s = new double[Math.Min(m + 1, n)];
            u = new double[m,nu];
            this.v = new double[n,n];
            double[] e = new double[n];
            double[] work = new double[m];

            // Reduce A to bidiagonal form, storing the diagonal elements
            // in s and the super-diagonal elements in e.
            int nct = Math.Min(m - 1, n);
            int nrt = Math.Max(0, Math.Min(n - 2, m));
            for (int k = 0; k < Math.Max(nct, nrt); k++)
            {
                if (k < nct)
                {
                    // Compute the transformation for the k-th column and
                    // place the k-th diagonal in s[k].
                    // Compute 2-norm of k-th column without under/overflow.
                    this.s[k] = 0;
                    for (int i = k; i < m; i++)
                    {
                        this.s[k] = Hypot(this.s[k], a[i, k]);
                    }
                    if (this.s[k] != 0.0)
                    {
                        if (a[k, k] < 0.0)
                        {
                            this.s[k] = -this.s[k];
                        }
                        for (int i = k; i < m; i++)
                        {
                            a[i, k] /= this.s[k];
                        }
                        a[k, k] += 1.0;
                    }
                    this.s[k] = -this.s[k];
                }
                for (int j = k + 1; j < n; j++)
                {
                    if ((k < nct) & (this.s[k] != 0.0))
                    {
                        // Apply the transformation.
                        double t = 0;

                        for (int i = k; i < m; i++)
                        {
                            t += a[i, k] * a[i, j];
                        }

                        t = -t / a[k, k];

                        for (int i = k; i < m; i++)
                        {
                            a[i, j] += t * a[i, k];
                        }
                    }

                    // Place the k-th row of A into e for the
                    // subsequent calculation of the row transformation.
                    e[j] = a[k, j];
                }
                if (withU & (k < nct))
                {
                    // Place the transformation in U for subsequent back
                    // multiplication.
                    for (int i = k; i < m; i++)
                    {
                        u[i, k] = a[i, k];
                    }
                }
                if (k < nrt)
                {
                    // Compute the k-th row transformation and place the
                    // k-th super-diagonal in e[k].
                    // Compute 2-norm without under/overflow.
                    e[k] = 0;

                    for (int i = k + 1; i < n; i++)
                    {
                        e[k] = Hypot(e[k], e[i]);
                    }

                    if (e[k] != 0.0)
                    {
                        if (e[k + 1] < 0.0)
                        {
                            e[k] = -e[k];
                        }
                        for (int i = k + 1; i < n; i++)
                        {
                            e[i] /= e[k];
                        }
                        e[k + 1] += 1.0;
                    }

                    e[k] = -e[k];

                    if ((k + 1 < m) & (e[k] != 0.0))
                    {
                        // Apply the transformation.
                        for (int i = k + 1; i < m; i++)
                        {
                            work[i] = 0.0;
                        }

                        for (int j = k + 1; j < n; j++)
                        {
                            for (int i = k + 1; i < m; i++)
                            {
                                work[i] += e[j] * a[i, j];
                            }
                        }

                        for (int j = k + 1; j < n; j++)
                        {
                            double t = -e[j] / e[k + 1];
                            for (int i = k + 1; i < m; i++)
                            {
                                a[i, j] += t * work[i];
                            }
                        }
                    }

                    if (withV)
                    {
                        // Place the transformation in V for subsequent
                        // back multiplication.
                        for (int i = k + 1; i < n; i++)
                        {
                            this.v[i, k] = e[i];
                        }
                    }
                }
            }

            // Set up the final bidiagonal matrix or order p.
            int p = Math.Min(n, m + 1);

            if (nct < n)
            {
                this.s[nct] = a[nct, nct];
            }
            if (m < p)
            {
                this.s[p - 1] = 0.0;
            }
            if (nrt + 1 < p)
            {
                e[nrt] = a[nrt, p - 1];
            }

            e[p - 1] = 0.0;

            // If required, generate U.
            if (withU)
            {
                for (int j = nct; j < nu; j++)
                {
                    for (int i = 0; i < m; i++)
                    {
                        u[i, j] = 0.0;
                    }
                    u[j, j] = 1.0;
                }

                for (int k = nct - 1; k >= 0; k--)
                {
                    if (this.s[k] != 0.0)
                    {
                        for (int j = k + 1; j < nu; j++)
                        {
                            double t = 0;
                            for (int i = k; i < m; i++)
                            {
                                t += u[i, k] * u[i, j];
                            }
                            t = -t / u[k, k];
                            for (int i = k; i < m; i++)
                            {
                                u[i, j] += t * u[i, k];
                            }
                        }
                        for (int i = k; i < m; i++)
                        {
                            u[i, k] = -u[i, k];
                        }
                        u[k, k] = 1.0 + u[k, k];
                        for (int i = 0; i < k - 1; i++)
                        {
                            u[i, k] = 0.0;
                        }
                    }
                    else
                    {
                        for (int i = 0; i < m; i++)
                        {
                            u[i, k] = 0.0;
                        }
                        u[k, k] = 1.0;
                    }
                }
            }

            // If required, generate V.
            if (withV)
            {
                for (int k = n - 1; k >= 0; k--)
                {
                    if ((k < nrt) & (e[k] != 0.0))
                    {
                        for (int j = k + 1; j < nu; j++)
                        {
                            double t = 0;

                            for (int i = k + 1; i < n; i++)
                            {
                                t += this.v[i, k] * this.v[i, j];
                            }

                            t = -t / this.v[k + 1, k];

                            for (int i = k + 1; i < n; i++)
                            {
                                this.v[i, j] += t * this.v[i, k];
                            }
                        }
                    }
                    for (int i = 0; i < n; i++)
                    {
                        this.v[i, k] = 0.0;
                    }

                    this.v[k, k] = 1.0;
                }
            }

            // Main iteration loop for the singular values.
            int pp = p - 1;
            int iter = 0;
            double eps = Math.Pow(2.0, -52.0);
            double tiny = Math.Pow(2.0, -966.0);

            while (p > 0)
            {
                int k, kase;

                // Here is where a test for too many iterations would go.

                // This section of the program inspects for
                // negligible elements in the s and e arrays.  On
                // completion the variables case and k are set as follows.

                // case = 1     if s(p) and e[k-1] are negligible and k<p
                // case = 2     if s(k) is negligible and k<p
                // case = 3     if e[k-1] is negligible, k<p, and
                //              s(k), ..., s(p) are not negligible (qr step).
                // case = 4     if e(p-1) is negligible (convergence).

                for (k = p - 2; k >= -1; k--)
                {
                    if (k == -1)
                    {
                        break;
                    }
                    if (Math.Abs(e[k]) <=
                        tiny + eps * (Math.Abs(this.s[k]) + Math.Abs(this.s[k + 1])))
                    {
                        e[k] = 0.0;
                        break;
                    }
                }
                if (k == p - 2)
                {
                    kase = 4;
                }
                else
                {
                    int ks;

                    for (ks = p - 1; ks >= k; ks--)
                    {
                        if (ks == k)
                        {
                            break;
                        }

                        double t = (ks != p ? Math.Abs(e[ks]) : 0) +
                                   (ks != k + 1 ? Math.Abs(e[ks - 1]) : 0);

                        if (Math.Abs(this.s[ks]) <= tiny + eps * t)
                        {
                            this.s[ks] = 0.0;
                            break;
                        }
                    }
                    if (ks == k)
                    {
                        kase = 3;
                    }
                    else if (ks == p - 1)
                    {
                        kase = 1;
                    }
                    else
                    {
                        kase = 2;
                        k = ks;
                    }
                }

                k++;

                // Perform the task indicated by case.
                switch (kase)
                {
                        // Deflate negligible s(p).
                    case 1:
                        {
                            double f = e[p - 2];

                            e[p - 2] = 0.0;

                            for (int j = p - 2; j >= k; j--)
                            {
                                double t = Hypot(this.s[j], f);
                                double cs = this.s[j] / t;
                                double sn = f / t;

                                this.s[j] = t;

                                if (j != k)
                                {
                                    f = -sn * e[j - 1];
                                    e[j - 1] = cs * e[j - 1];
                                }
                                if (withV)
                                {
                                    for (int i = 0; i < n; i++)
                                    {
                                        t = cs * this.v[i, j] + sn * this.v[i, p - 1];
                                        this.v[i, p - 1] = -sn * this.v[i, j] + cs * v[i, p - 1];
                                        v[i, j] = t;
                                    }
                                }
                            }
                        }
                        break;

                        // Split at negligible s(k).
                    case 2:
                        {
                            double f = e[k - 1];

                            e[k - 1] = 0.0;

                            for (int j = k; j < p; j++)
                            {
                                double t = Hypot(this.s[j], f);
                                double cs = this.s[j] / t;
                                double sn = f / t;

                                this.s[j] = t;
                                f = -sn * e[j];
                                e[j] = cs * e[j];

                                if (withU)
                                {
                                    for (int i = 0; i < m; i++)
                                    {
                                        t = cs * u[i, j] + sn * u[i, k - 1];
                                        u[i, k - 1] = -sn * u[i, j] + cs * u[i, k - 1];
                                        u[i, j] = t;
                                    }
                                }
                            }
                        }
                        break;

                        // Perform one qr step.
                    case 3:
                        {
                            // Calculate the shift.
                            double scale = Math.Max(Math.Max(Math.Max(Math.Max(
                                                                          Math.Abs(this.s[p - 1]),
                                                                          Math.Abs(this.s[p - 2])), Math.Abs(e[p - 2])),
                                                             Math.Abs(this.s[k])), Math.Abs(e[k]));
                            double sp = this.s[p - 1] / scale;
                            double spm1 = this.s[p - 2] / scale;
                            double epm1 = e[p - 2] / scale;
                            double sk = this.s[k] / scale;
                            double ek = e[k] / scale;
                            double b = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) / 2.0;
                            double c = (sp * epm1) * (sp * epm1);
                            double shift = 0.0;

                            if ((b != 0.0) | (c != 0.0))
                            {
                                shift = Math.Sqrt(b * b + c);
                                if (b < 0.0)
                                {
                                    shift = -shift;
                                }
                                shift = c / (b + shift);
                            }

                            double f = (sk + sp) * (sk - sp) + shift;
                            double g = sk * ek;

                            // Chase zeros.
                            for (int j = k; j < p - 1; j++)
                            {
                                double t = Hypot(f, g);
                                double cs = f / t;
                                double sn = g / t;

                                if (j != k)
                                {
                                    e[j - 1] = t;
                                }
                                f = cs * this.s[j] + sn * e[j];
                                e[j] = cs * e[j] - sn * this.s[j];
                                g = sn * this.s[j + 1];
                                this.s[j + 1] = cs * this.s[j + 1];

                                if (withV)
                                {
                                    for (int i = 0; i < n; i++)
                                    {
                                        t = cs * this.v[i, j] + sn * this.v[i, j + 1];
                                        this.v[i, j + 1] = -sn * this.v[i, j] + cs * this.v[i, j + 1];
                                        this.v[i, j] = t;
                                    }
                                }
                                t = Hypot(f, g);
                                cs = f / t;
                                sn = g / t;
                                this.s[j] = t;
                                f = cs * e[j] + sn * this.s[j + 1];
                                this.s[j + 1] = -sn * e[j] + cs * this.s[j + 1];
                                g = sn * e[j + 1];
                                e[j + 1] = cs * e[j + 1];

                                if (withU && (j < m - 1))
                                {
                                    for (int i = 0; i < m; i++)
                                    {
                                        t = cs * this.u[i, j] + sn * this.u[i, j + 1];
                                        this.u[i, j + 1] = -sn * this.u[i, j] + cs * this.u[i, j + 1];
                                        this.u[i, j] = t;
                                    }
                                }
                            }
                            e[p - 2] = f;
                            iter = iter + 1;
                        }
                        break;

                        // Convergence.
                    case 4:
                        {
                            // Make the singular values positive.
                            if (this.s[k] <= 0.0)
                            {
                                this.s[k] = (this.s[k] < 0.0 ? -this.s[k] : 0.0);

                                if (withV)
                                {
                                    for (int i = 0; i <= pp; i++)
                                    {
                                        this.v[i, k] = -this.v[i, k];
                                    }
                                }
                            }

                            // Order the singular values.
                            while (k < pp)
                            {
                                if (this.s[k] >= this.s[k + 1])
                                {
                                    break;
                                }

                                double t = this.s[k];

                                this.s[k] = this.s[k + 1];
                                this.s[k + 1] = t;

                                if (withV && (k < n - 1))
                                {
                                    for (int i = 0; i < n; i++)
                                    {
                                        t = this.v[i, k + 1];
                                        this.v[i, k + 1] = this.v[i, k];
                                        this.v[i, k] = t;
                                    }
                                }
                                if (withU && (k < m - 1))
                                {
                                    for (int i = 0; i < m; i++)
                                    {
                                        t = this.u[i, k + 1];
                                        this.u[i, k + 1] = this.u[i, k];
                                        this.u[i, k] = t;
                                    }
                                }

                                k++;
                            }

                            iter = 0;
                            p--;
                        }

                        break;
                }
            }
        }

        /// <summary>
        /// Euclidean distance function.
        /// </summary>
        /// <param name="a">The first value.</param>
        /// <param name="b">The second value.</param>
        /// <returns>The computed distance.</returns>
        private static double Hypot(double a, double b)
        {
            double r;

            if (Math.Abs(a) > Math.Abs(b))
            {
                r = b / a;
                r = Math.Abs(a) * Math.Sqrt(1 + r * r);
            }
            else if (b != 0)
            {
                r = a / b;
                r = Math.Abs(b) * Math.Sqrt(1 + r * r);
            }
            else
            {
                r = 0.0;
            }

            return r;
        }
    }
}